#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_listener");
    ros::NodeHandle node;
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
    // TF监听器
    tf::TransformListener listener; // 缓存十秒
    ros::Rate rate(10.0);
    while (node.ok())
    {
        tf::StampedTransform transform;
        rate.sleep();
        try
        {
            // 查找turtle2和turtle1的坐标变换
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); //target_frame, source_frame
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        // 根据tuetle1和turtle2之间的坐标变换，计算turtle2运动需要的线速度和角速度
        // 并发布速度控制指令，使turtle2向turtle1移动
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);        
    }
    return 0;
}


/* #include "ros/ros.h"
#include  "geometry_msgs/Twist.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
int main(int argc, char *argv[])
{
        setlocale(LC_ALL,"");

        ros::init(argc,argv,"turtle_sub");
        ros::NodeHandle nh;

        ros::Publisher  pub  = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);

        geometry_msgs::Twist twist;

        ros::Rate r(10);

        while (ros::ok())
        {
                try
                {       
                        tf2_ros::Buffer buff;
                        tf2_ros::TransformListener lis(buff);
                        
                        ros::Duration(3.0);
                        geometry_msgs::TransformStamped tfs =   buff.lookupTransform("turtle2","turtle1",ros::Time(0));
                        
                        twist.linear.x = 0.3 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));;
                        twist.angular.z = 4 *  atan2(tfs.transform.translation.y,tfs.transform.translation.x);
                        pub.publish(twist);
                }
                catch(const std::exception& e)
                {
                        ROS_INFO("%s",e.what());
                }

                ros::spinOnce();
                r.sleep();
        }
        return 0;
} */